/**********************************************************************
*  Copyright (c) 2007 - Victor Jacobs - victor.jacobs@gmail.com
*
*  Permission is hereby granted, free of charge, to any person
*  obtaining a copy of this software and associated documentation
*  files (the "Software"), to deal in the Software without
*  restriction, including without limitation the rights to use,
*  copy, modify, merge, publish, distribute, sublicense, and/or sell
*  copies of the Software, and to permit persons to whom the
*  Software is furnished to do so, subject to the following
*  conditions:
*
*  The above copyright notice and this permission notice shall be
*  included in all copies or substantial portions of the Software.
*
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
*  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
*  OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
*  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
*  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
*  WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
*  OTHER DEALINGS IN THE SOFTWARE.
**********************************************************************/

using System;
using System.Collections.Generic;
using System.Text;
using Tao.Ode;
using Tao.OpenGl;
using Vj.ShapeLibrary;
using Vj.MathLibrary;
using Vj.ODEObjectLibrary;
using System.Drawing;

namespace Vj.OdeObjectLibrary
{
    public enum Side
    {
        Left, Right
    }

    public enum LegPositions
    {
        Front, Mid, Back
    }

    public enum JointPosition
    {
        Upper, Lower
    }

    public class Leg : ODEObject
    {
        #region Properties

        private Ode.dMatrix3 upperRotation;
        private Ode.dMatrix3 lowerRotation;

        private Ode.dVector3 upperDimension;

        public Ode.dVector3 UpperDimension
        {
            get { return upperDimension; }
            set { upperDimension = value; }
        }

        private Ode.dVector3 lowerDimension;

        public Ode.dVector3 LowerDimension
        {
            get { return lowerDimension; }
            set { lowerDimension = value; }
        }

        private Ode.dVector3 upperLocation;

        public Ode.dVector3 UpperLocation
        {
            get { return upperLocation; }
            set { upperLocation = value; }
        }

        private Ode.dVector3 lowerLocation;

        public Ode.dVector3 LowerLocation
        {
            get { return lowerLocation; }
            set { lowerLocation = value; }
        }

        private Side mySide;

        public Side Side
        {
            get { return mySide; }
        }


        #endregion

        public Leg(IntPtr world, IntPtr space, Side side, float mass,
            Ode.dVector3 centerPosition, Color color)
        {
            this.mySide = side;
            this.mass = mass;
            this.color = color;
            this.position = centerPosition;

            bodyArray = new IntPtr[2];
            geomArray = new IntPtr[2];
            jointArray = new IntPtr[1];

            if (side == Side.Right)
            {
                upperLocation =
                    ODEUtility.Add(centerPosition, new Ode.dVector3(-1.0f, 0.0f, 0.0f));

                lowerLocation =
                    ODEUtility.Add(centerPosition, new Ode.dVector3(0.5f, 0.0f, 0.0f));
            }
            else
            {
                upperLocation =
                    ODEUtility.Add(centerPosition, new Ode.dVector3(1.0f, 0.0f, 0.0f));

                lowerLocation =
                    ODEUtility.Add(centerPosition, new Ode.dVector3(-0.5f, 0.0f, 0.0f));
            }


            upperDimension = new Ode.dVector3(0.95f, 0.25f, 0.25f);
            lowerDimension = new Ode.dVector3(1.9f, 0.1f, 0.1f);

           
            bodyArray[0] = Ode.dBodyCreate(world);
            Ode.dBodySetPosition(bodyArray[0], upperLocation.X, upperLocation.Y, upperLocation.Z);
            geomArray[0] = Ode.dCreateBox(space, upperDimension.X, upperDimension.Y, upperDimension.Z);
            Ode.dGeomSetBody(geomArray[0], bodyArray[0]);

            Ode.dMass objMass = new Ode.dMass();
            Ode.dMassSetBox(ref objMass, 10, upperDimension.X, upperDimension.Y, upperDimension.Z);
            objMass.mass = 1;
            Ode.dBodySetMass(bodyArray[0], ref objMass);

            bodyArray[1] = Ode.dBodyCreate(world);
            Ode.dBodySetPosition(bodyArray[1], lowerLocation.X, lowerLocation.Y, lowerLocation.Z);
            geomArray[1] = Ode.dCreateBox(space, lowerDimension.X, lowerDimension.Y, lowerDimension.Z);
            Ode.dGeomSetBody(geomArray[1], bodyArray[1]);

            Ode.dMass objMass2 = new Ode.dMass();
            Ode.dMassSetBox(ref objMass2, 10, lowerDimension.X, lowerDimension.Y, lowerDimension.Z);
            objMass2.mass = 1;
            Ode.dBodySetMass(bodyArray[1], ref objMass2);


            Ode.dVector3 jointPosition = new Ode.dVector3();

            if (this.Side == Side.Right)
                jointPosition.X = LowerLocation.X - LowerDimension.X / 2.0f;
            else
                jointPosition.X = LowerLocation.X + LowerDimension.X / 2.0f;

            jointPosition.Y = LowerLocation.Y;
            jointPosition.Z = LowerLocation.Z;

            jointArray[0] = Ode.dJointCreateHinge(world, new IntPtr(0));
            Ode.dJointAttach(jointArray[0], bodyArray[0], bodyArray[1]);
            Ode.dJointSetHingeAnchor(jointArray[0], jointPosition.X, jointPosition.Y, jointPosition.Z);
            
            if(side == Side.Right)
                Ode.dJointSetHingeAxis(jointArray[0], 0.0f, 0.0f, 1.0f);
            else
                Ode.dJointSetHingeAxis(jointArray[0], 0.0f, 0.0f, -1.0f);

        }



        public override void Render()
        {
            float[] mat_ambient = { 0.25f * (float)color.R / 255.0f, 0.25f * (float)color.G / 255.0f, 0.25f * (float)color.B / 255.0f, 1.0f };
            float[] mat_diffuse = { 0.6f * (float)color.R / 255.0f, 0.6f * (float)color.G / 255.0f, 0.6f * (float)color.B / 255.0f, 1.0f };
            float[] mat_specular ={ 0.2f * (float)color.R / 255.0f, 0.2f * (float)color.G / 255.0f, 0.2f * (float)color.B / 255.0f, 1.0f };
            float[] mat_shininess = { 0.6f };

            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_SPECULAR, mat_specular);
            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_DIFFUSE, mat_diffuse);
            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_AMBIENT, mat_ambient);
            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_AMBIENT, mat_shininess);

            upperLocation = Ode.dGeomGetPosition(this.geomArray[0]);
            upperRotation = Ode.dGeomGetRotation(this.geomArray[0]);

            Gl.glPushMatrix();

            Gl.glMultMatrixf(ODEUtility.ODE_Rotation_Position_to_OpenGL_FloatArray(upperRotation, upperLocation));

            Box.Draw(new Vector(upperDimension.X, upperDimension.Y, upperDimension.Z),
                     color);

            Gl.glPopMatrix();


            //float[] mat_ambient2 = { 0.25f * (float)color.X, 0.25f * (float)color.Y, 0.25f * (float)color.Z, 1.0f };
            //float[] mat_diffuse2 = { 0.6f * (float)color.X, 0.6f * (float)color.Y, 0.6f * (float)color.Z, 1.0f };
            //float[] mat_specular2 ={ 0.2f * (float)color.X, 0.2f * (float)color.Y, 0.2f * (float)color.Z, 1.0f };
            //float[] mat_shininess2 = { 0.1f };
            
            //Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_SPECULAR, mat_specular2);
            //Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_DIFFUSE, mat_diffuse2);
            //Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_AMBIENT, mat_ambient2);
            //Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_AMBIENT, mat_shininess2);


            lowerLocation = Ode.dGeomGetPosition(this.geomArray[1]);
            lowerRotation = Ode.dGeomGetRotation(this.geomArray[1]);

            Gl.glPushMatrix();

            Gl.glMultMatrixf(ODEUtility.ODE_Rotation_Position_to_OpenGL_FloatArray(lowerRotation, lowerLocation));

            Box.Draw(new Vector(lowerDimension.X, lowerDimension.Y, lowerDimension.Z),
                     color);

            Gl.glPopMatrix();
        }

        public void AddTorque(float torque)
        {
            Ode.dJointAddHingeTorque(this.jointArray[0], torque);
        }

    }

    public class Hexapod : ODEObject
    {
        private float size;

        private Leg[] legs;

        private Ode.dVector3 bodyDimension;
        private Ode.dVector3 bodyPosition;
        private Ode.dMatrix3 bodyRotation;

        #region Constructors

        public Hexapod(IntPtr world, IntPtr space, float size,
            float mass, Ode.dVector3 position, Color color)
        {
            this.position = position;
            this.color = color;
            this.mass = mass;

            this.size = size;

            bodyArray = new IntPtr[1];
            geomArray = new IntPtr[1];
            jointArray = new IntPtr[6];

            legs = new Leg[8];

            legs[0] = new Leg(world, space, Side.Right, 1.0f,
                ODEUtility.Add(position, new Ode.dVector3(2.5f, 0.0f, 1.0f)),
                //new Ode.dVector3(1.5f, 2.0f, 1.0f), new Ode.dVector3(3.0f, 2.0f, 1.0f),
                color);

            legs[1] = new Leg(world, space, Side.Right, 1.0f,
                ODEUtility.Add(position, new Ode.dVector3(2.5f, 0.0f, 0.0f)),
                //new Ode.dVector3(1.5f, 2.0f, 0.0f), new Ode.dVector3(3.0f, 2.0f, 0.0f),
                color);

            legs[2] = new Leg(world, space, Side.Right, 1.0f,
                ODEUtility.Add(position, new Ode.dVector3(2.5f, 0.0f, -1.0f)),
                //new Ode.dVector3(1.5f, 2.0f, -1.0f), new Ode.dVector3(3.0f, 2.0f, -1.0f),
                color);

            legs[3] = new Leg(world, space, Side.Left, 1.0f,
                ODEUtility.Add(position, new Ode.dVector3(-2.5f, 0.0f, 1.0f)),
                //new Ode.dVector3(-1.5f, 2.0f, 1.0f), new Ode.dVector3(-3.0f, 2.0f, 1.0f),
                color);

            legs[4] = new Leg(world, space, Side.Left, 1.0f,
                ODEUtility.Add(position, new Ode.dVector3(-2.5f, 0.0f, 0.0f)),
                //new Ode.dVector3(-1.5f, 2.0f, 0.0f), new Ode.dVector3(-3.0f, 2.0f, 0.0f),
                color);

            legs[5] = new Leg(world, space, Side.Left, 1.0f,
                ODEUtility.Add(position, new Ode.dVector3(-2.5f, 0.0f, -1.0f)),
                //new Ode.dVector3(-1.5f, 2.0f, -1.0f), new Ode.dVector3(-3.0f, 2.0f, -1.0f),
                color);

            bodyDimension = new Ode.dVector3(1.95f, 0.25f, 3.0f);
            bodyPosition = position; //new Ode.dVector3(0.0f, 2.0f, 0.0f);
            
            bodyArray[0] = Ode.dBodyCreate(world);
            Ode.dBodySetPosition(bodyArray[0], bodyPosition.X, bodyPosition.Y, bodyPosition.Z);
            geomArray[0] = Ode.dCreateBox(space, bodyDimension.X, bodyDimension.Y, bodyDimension.Z);
            Ode.dGeomSetBody(geomArray[0], bodyArray[0]);

            Ode.dMass objMass = new Ode.dMass();
            Ode.dMassSetBox(ref objMass, 10, bodyDimension.X, bodyDimension.Y, bodyDimension.Z);
            objMass.mass = 1;
            Ode.dBodySetMass(bodyArray[0], ref objMass);

            for (int i = 0; i < 6; i++)
            {
                Ode.dVector3 jointPosition = new Ode.dVector3();

                if(legs[i].Side == Side.Right)
                    jointPosition.X = legs[i].UpperLocation.X - legs[i].UpperDimension.X / 2.0f;
                else
                    jointPosition.X = legs[i].UpperLocation.X + legs[i].UpperDimension.X / 2.0f;
                
                jointPosition.Y = legs[i].UpperLocation.Y;
                jointPosition.Z = legs[i].UpperLocation.Z;

                #region Other JointTypes
                //jointArray[i] = Ode.dJointCreateBall(world, new IntPtr(0));
                //Ode.dJointAttach(jointArray[i], bodyArray[0], legs[i].BodyArray[0]);
                //Ode.dJointSetBallAnchor(jointArray[i], jointPosition.X, jointPosition.Y, jointPosition.Z);
                //Ode.dJointSetBallAnchor2(jointArray[i], jointPosition.X, jointPosition.Y, jointPosition.Z);

                //jointArray[i] = Ode.dJointCreateHinge(world, new IntPtr(0));
                //Ode.dJointAttach(jointArray[i], bodyArray[0], legs[i].BodyArray[0]);
                //Ode.dJointSetHingeAnchor(jointArray[i], jointPosition.X, jointPosition.Y, jointPosition.Z);
                //Ode.dJointSetHingeAxis(jointArray[i], 0.0f, 0.0f, 1.0f);
                #endregion  

                jointArray[i] = Ode.dJointCreateUniversal(world, new IntPtr(0));
                Ode.dJointAttach(jointArray[i], bodyArray[0], legs[i].BodyArray[0]);
                Ode.dJointSetUniversalAnchor(jointArray[i], jointPosition.X, jointPosition.Y, jointPosition.Z);

                if (legs[i].Side == Side.Right)
                {
                    Ode.dJointSetUniversalAxis1(jointArray[i], 0.0f, 0.0f, 1.0f);
                    Ode.dJointSetUniversalAxis2(jointArray[i], 0.0f, 1.0f, 1.0f);
                }
                else
                {
                    Ode.dJointSetUniversalAxis1(jointArray[i], 0.0f, 0.0f, -1.0f);
                    Ode.dJointSetUniversalAxis2(jointArray[i], 0.0f, 1.0f, -1.0f);                
                }
            }
        }

        #endregion

        public override void Render()
        {
            float[] mat_ambient = { 0.25f * (float)color.R / 255.0f, 0.25f * (float)color.G / 255.0f, 0.25f * (float)color.B / 255.0f, 1.0f };
            float[] mat_diffuse = { 0.6f * (float)color.R / 255.0f, 0.6f * (float)color.G / 255.0f, 0.6f * (float)color.B / 255.0f, 1.0f };
            float[] mat_specular ={ 0.2f * (float)color.R / 255.0f, 0.2f * (float)color.G / 255.0f, 0.2f * (float)color.B / 255.0f, 1.0f };
            float[] mat_shininess = { 0.1f };

            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_SPECULAR, mat_specular);
            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_DIFFUSE, mat_diffuse);
            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_AMBIENT, mat_ambient);
            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_AMBIENT, mat_shininess);

            bodyPosition = Ode.dGeomGetPosition(this.geomArray[0]);
            bodyRotation = Ode.dGeomGetRotation(this.geomArray[0]);

            Gl.glPushMatrix();

            Gl.glMultMatrixf(ODEUtility.ODE_Rotation_Position_to_OpenGL_FloatArray(bodyRotation, bodyPosition));

            Box.Draw(new Vector(bodyDimension.X, bodyDimension.Y, bodyDimension.Z),
                     color);

            Gl.glPopMatrix();

            for (int i = 0; i < 6; i++)
            {
                legs[i].Render();
            }


            //Ray.Draw(new Vector(0.0, 0.0, 0.0), 
            //    new Vector(1.0, 2.0, 1.0));

        }

        public void SetJointTorque(LegPositions legPos, Side side, JointPosition jPos, float [] torques)
        {
            if (jPos == JointPosition.Upper && torques.Length != 2)
                throw new Exception("Invalid number of torque values.  Upper joint requires two torque values.");

            if (jPos == JointPosition.Lower && torques.Length != 1)
                throw new Exception("Invalid number of torque values.  Lower joint requires one torque value.");

            // Right side legs

            // Upper Joint
            if (legPos == LegPositions.Front && side == Side.Right && jPos == JointPosition.Upper)
            {
                Ode.dJointAddUniversalTorques(jointArray[0], torques[0], torques[1]);
            }
            else if (legPos == LegPositions.Mid && side == Side.Right && jPos == JointPosition.Upper)
            {
                Ode.dJointAddUniversalTorques(jointArray[1], torques[0], torques[1]);
            }
            else if (legPos == LegPositions.Back && side == Side.Right && jPos == JointPosition.Upper)
            {
                Ode.dJointAddUniversalTorques(jointArray[2], torques[0], torques[1]);
            }

            // Lower Joint
            if (legPos == LegPositions.Front && side == Side.Right && jPos == JointPosition.Lower)
            {
                legs[0].AddTorque(torques[0]);
            }
            else if (legPos == LegPositions.Mid && side == Side.Right && jPos == JointPosition.Lower)
            {
                legs[1].AddTorque(torques[0]);
            }
            else if (legPos == LegPositions.Back && side == Side.Right && jPos == JointPosition.Lower)
            {
                legs[2].AddTorque(torques[0]);
            }

            // Left side legs
            
            // Upper
            else if (legPos == LegPositions.Front && side == Side.Left && jPos == JointPosition.Upper)
            {
                Ode.dJointAddUniversalTorques(jointArray[3], torques[0], torques[1]);
            }
            else if (legPos == LegPositions.Mid && side == Side.Left && jPos == JointPosition.Upper)
            {
                Ode.dJointAddUniversalTorques(jointArray[4], torques[0], torques[1]);
            }
            else if (legPos == LegPositions.Back && side == Side.Left && jPos == JointPosition.Upper)
            {
                Ode.dJointAddUniversalTorques(jointArray[5], torques[0], torques[1]);
            }

            // Lower
            else if (legPos == LegPositions.Front && side == Side.Left && jPos == JointPosition.Lower)
            {
                legs[3].AddTorque(torques[0]);
            }
            else if (legPos == LegPositions.Mid && side == Side.Left && jPos == JointPosition.Lower)
            {
                legs[4].AddTorque(torques[0]);
            }
            else if (legPos == LegPositions.Back && side == Side.Left && jPos == JointPosition.Lower)
            {
                legs[5].AddTorque(torques[0]);
            }






        }

    }
}
